#include <vector>
#include <iostream>

#include <ros/ros.h>
#include <std_srvs/Empty.h>
#include <nav_msgs/GetPlan.h>
#include <geometry_msgs/PoseStamped.h>
#include <windows_communicator/MzNavState.h>

#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>

#include <tf/tf.h>
#include "tf/transform_listener.h"
#include "tf/transform_broadcaster.h"

enum WorkingState{
    STOP = 0,                           // 停止
    MOVE_WITH_FACE_DETECT = 1,          // 需要保证目标人脸离摄像头不远才进行导航
    MOVE_WITHOUT_FACE_DETECT = 2,       // 无脑导航
};


class BusinessLogic {
public:
    BusinessLogic();
private:
    ros::NodeHandle nh;
    ros::ServiceClient make_plan_srv_client;
    ros::ServiceClient clear_costmaps_srv_client;
    std::vector<geometry_msgs::PoseStamped> path_points;
    actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> action_client;  // 在声明中不可使用非默认构造函数，只能在初始化列表中调用显式构造函数

    WorkingState working_state = STOP;
    float target_x = 0, target_y = 0, target_theta = 0;
    float face_ratio = 0;
    bool target_change_flag = 0;  // 用于标记目标点切换，需要重新call movebase

	void NavStateCallback(const windows_communicator::MzNavState &msg);
    void GetBaselinkPos(double& x, double& y, double& yaw);
    void UpdatePathPlanning(const double x_goal, const double y_goal, const double yaw_goal);
};

BusinessLogic::BusinessLogic() : action_client("move_base", true){
    const int LOOP_RATE = 10;                   // [HZ]
    const float LOOP_TIME = 1.0/LOOP_RATE;      // [s]
    const float FACE_RATIO_THRESHOLD = 0.1;     // [ratio]
    const float NO_FACE_TIME_THRESHOLD = 5.0;   // [s]
    static float no_face_time_sum = 0;

	make_plan_srv_client = nh.serviceClient<nav_msgs::GetPlan>("move_base/make_plan");
	clear_costmaps_srv_client = nh.serviceClient<std_srvs::Empty>("move_base/clear_costmaps");
    ros::Subscriber subber = nh.subscribe("nav_state", 1, &BusinessLogic::NavStateCallback, this);
    ros::Rate r(LOOP_RATE);
    while (ros::ok()) {
        // 外层循环根据当前target坐标调用movebase的action
        if(working_state != STOP && \
           !(working_state == MOVE_WITH_FACE_DETECT && face_ratio < FACE_RATIO_THRESHOLD)) {  // 在两个条件下不开始导航
            // 判断是否已在目标点附近
            double x, y, yaw;
            GetBaselinkPos(x, y, yaw);
            double dis = hypot(x-target_x, y-target_y);
            // 参数服务器读取参数 
            double xy_goal_tolerance, yaw_goal_tolerance;
            ros::param::param<double>("/move_base/EBandPlannerROS/xy_goal_tolerance", xy_goal_tolerance, 0.3);
            ros::param::param<double>("/move_base/EBandPlannerROS/yaw_goal_tolerance", yaw_goal_tolerance, 0.3);
            if(dis<xy_goal_tolerance && abs(yaw-target_theta)<yaw_goal_tolerance) {  // 若过近则认为已到达,不进行后续逻辑
                r.sleep();
                ros::spinOnce();
                continue;
            }
            // 开始导航
            no_face_time_sum = 0;
            target_change_flag = 0;
            move_base_msgs::MoveBaseGoal goal;
            geometry_msgs::Quaternion quaternion = tf::createQuaternionMsgFromYaw(target_theta);
            goal.target_pose.header.frame_id = "map";
            goal.target_pose.header.stamp = ros::Time::now();
            goal.target_pose.pose.position.x = target_x;
            goal.target_pose.pose.position.y = target_y;
            goal.target_pose.pose.orientation = quaternion;
            action_client.sendGoal(goal);
            ROS_INFO("[BusinessLogic] Start Call Movebase Acion, Target:x[%.3f], y[%.3f], yaw[%.3f]", target_x, target_y, target_theta);
            while(ros::ok()) {
                // 内层循环判断当前状态是否应该继续导航
                action_client.waitForResult(ros::Duration(LOOP_TIME));
                if(action_client.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
                    ROS_INFO("[BusinessLogic] Target Position Arrived!");
                    break;
                } else if(working_state == STOP) {
                    ROS_INFO("[BusinessLogic] State Change to STOP");
                    action_client.cancelAllGoals();
                    break;
                } else if(target_change_flag == 1){
                    ROS_INFO("[BusinessLogic] Target Change");
                    action_client.cancelAllGoals();
                    break;
                } else if(working_state == MOVE_WITH_FACE_DETECT) {  
                    // 工作在人脸检测模式下时若一段时间未看到足够大的目标人脸，则停下等待
                    if(face_ratio < FACE_RATIO_THRESHOLD) no_face_time_sum += LOOP_TIME;
                    else no_face_time_sum = 0;
                    if(no_face_time_sum > NO_FACE_TIME_THRESHOLD) {
                        ROS_WARN("[BusinessLogic] Target Face Undetected for Too Long Time, Stop and Wait for Face");
                        action_client.cancelAllGoals();
                        break;
                    }
                } else if(action_client.getState() == actionlib::SimpleClientGoalState::ABORTED) {
                    ROS_WARN("[BusinessLogic] Aborted! Clear Costmaps and Try Again.");
                    std_srvs::Empty srv;
                    clear_costmaps_srv_client.call(srv);
                    break;
                }
                ros::spinOnce();
            }
        }
        r.sleep();
        ros::spinOnce();
    }
}

// 导航状态话题回调
void BusinessLogic::NavStateCallback(const windows_communicator::MzNavState &msg){
    working_state = (WorkingState) msg.working_state;
    if(target_x != msg.pos_x || target_y != msg.pos_y || target_theta != msg.pos_theta) target_change_flag = 1;
    target_x = msg.pos_x;
    target_y = msg.pos_y;
    target_theta = msg.pos_theta;
    face_ratio = msg.face_ratio;
}

// 获取当前map到base_link的坐标变换
void BusinessLogic::GetBaselinkPos(double& x, double& y, double& yaw) {
    tf::StampedTransform transform;
    tf::TransformListener* tf_;
    try {
        tf_ = new tf::TransformListener();
        tf_->waitForTransform("map", "base_link", ros::Time(0), ros::Duration(3.0));
        tf_->lookupTransform("map", "base_link", ros::Time(0), transform);
    }
    catch(tf::TransformException e) {
        ROS_WARN("[BusinessLogic] Failed to compute odom pose, skipping scan (%s)", e.what());
        return;
    }
    x = transform.getOrigin().x();
    y = transform.getOrigin().y();
    tf::Quaternion q = transform.getRotation();
    double useless_roll, useless_pitch;
    tf::Matrix3x3(q).getRPY(useless_roll, useless_pitch, yaw);
}

// 根据给入的目标点重新规划path_points(此函数只在获取路径规划结果时调用，常规逻辑用不到)
void BusinessLogic::UpdatePathPlanning(const double x_goal, const double y_goal, const double yaw_goal) {
    const double INTERVAL = 3.5;

    nav_msgs::GetPlan srv;
    double x, y, yaw;
    GetBaselinkPos(x, y, yaw);
    srv.request.start.header.frame_id ="map";
    srv.request.start.pose.position.x = x;          // 初始位置坐标
    srv.request.start.pose.position.y = y;
    srv.request.start.pose.orientation.w = yaw;
    srv.request.goal.header.frame_id = "map";
    srv.request.goal.pose.position.x = x_goal;      // 终点坐标
    srv.request.goal.pose.position.y = y_goal;
    srv.request.goal.pose.orientation.w = yaw_goal;

    /* 请求move_base/make_plan服务,获取原始路径规划结果并稀疏化 */
    std::vector<geometry_msgs::PoseStamped> org_path_points;
    if (make_plan_srv_client.call(srv)) {
        if (!srv.response.plan.poses.empty()) {
            for(size_t i = 1; i < (srv.response.plan.poses.size()); i++)
                org_path_points.push_back(srv.response.plan.poses[i]);
            /* 将org_path_points按照INTERVAL稀疏化后存入path_points */
            path_points.clear();
            if(org_path_points.size() >= 3) {
                geometry_msgs::PoseStamped last_point = org_path_points[0];
                path_points.push_back(last_point);
                for(size_t i = 1; i < (org_path_points.size()-1); i++) {
                    geometry_msgs::PoseStamped pose = org_path_points[i];
                    double dis = hypot(last_point.pose.position.x-pose.pose.position.x, last_point.pose.position.y-pose.pose.position.y);
                    if(dis >= INTERVAL) {
                        last_point = pose;
                        path_points.push_back(last_point);
                    }
                }
                path_points.push_back(org_path_points[org_path_points.size()-1]);
            } else {
                path_points.push_back(org_path_points[0]);
                path_points.push_back(org_path_points[1]);
            }
        } else {
            ROS_WARN("[BusinessLogic] Got empty plan");
        }
    } else {
        ROS_ERROR("[BusinessLogic] Failed to call service %s - is the robot moving?", make_plan_srv_client.getService().c_str());
    }
}


int main(int argc, char** argv) {
    ros::init(argc, argv, "business_logic");
    BusinessLogic foo;
}
